#include "OwnGraspTaskSimulator.hpp"

#include <ode/ode.h>

#include <rwlibs/simulation/SimulatedController.hpp>
#include <rwsim/drawable/SimulatorDebugRender.hpp>
#include <rwlibs/opengl/Drawable.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyFactory.hpp>
#include <rw/graspplanning/Grasp3D.hpp>
#include <rw/geometry/QHullND.hpp>
#include <rwlibs/task.hpp>
#include <fstream>
#include <iostream>
#include <stack>
#include <boost/lexical_cast.hpp>
#include <rwsim/dynamics/DynamicUtil.hpp>
#include <rw/graspplanning/CMDistCCPMeasure3D.hpp>
#include <rw/geometry/GeometryUtil.hpp>
#include <rwsim/dynamics/KinematicBody.hpp>
#include <rwsimlibs/ode/ODESimulator.hpp>
#include <rwsim/simulator/DynamicSimulator.hpp>
#include <rwsim/sensor/BodyContactSensor.hpp>
#include "OwnGWSMeasure3D.hpp"

using namespace rw::math;
using namespace rw::common;
using namespace rw::geometry;
using namespace rw::kinematics;
using namespace rw::proximity;
using namespace rwlibs::task;
using namespace rwlibs::proximitystrategies;
using namespace rwsim::dynamics;
using namespace rwsim::simulator;
using namespace rwsim::control;
using namespace rwsim::sensor;
using namespace rw::sensor;
using rw::graspplanning::CMDistCCPMeasure3D;
using rw::geometry::GeometryUtil;
using rw::graspplanning::Grasp3D;
using rwlibs::simulation::SimulatedController;

const int NR_OF_QUALITY_MEASURES = 5;

namespace {
double getMaxObjectDistance(std::vector<RigidBody*> objects, const State& s1, const State& s2){
	double max = 0;
	BOOST_FOREACH(RigidBody *object, objects){
		Transform3D<> t1 = object->getTransformW(s1);
		Transform3D<> t2 = object->getTransformW(s2);
		if(MetricUtil::dist2(t1.P(),t2.P())>max)
			max = MetricUtil::dist2(t1.P(),t2.P());
	}
	return max;
}
}


OwnGraspTaskSimulator::OwnGraspTaskSimulator(rwsim::dynamics::DynamicWorkCell::Ptr dwc):
										_dwc(dwc),
										_requestSimulationStop(false),
										_autoSaveInterval(40),
										_currentTargetIndex(0)
{
	_maxObjectGripperDistanceThreshold = 50;
	_stat.push_back(SimulationFailure);
	_initialized = false;
	_nrOfThreads = 1;
	_stepDelayMs = 0;
}

void OwnGraspTaskSimulator::init(rwsim::dynamics::DynamicWorkCell::Ptr dwc, const rw::kinematics::State& initState){
	_dwc = dwc;
	_collisionDetector = ownedPtr(
			new CollisionDetector( dwc->getWorkcell(), ProximityStrategyFactory::makeDefaultCollisionStrategy()));

	// initialize simulators
	_simStates.clear();
	_simulators.clear();

	for(int i=0;i<_nrOfThreads;i++){
		/*dRandSetSeed(0);
		srand(0);
		Math::seed(0);*/
		Log::debugLog() << "Making physics engine";
		ODESimulator::Ptr engine = ownedPtr( new ODESimulator(_dwc));
		Log::debugLog() << "Making simulator";
		DynamicSimulator::Ptr sim = ownedPtr( new DynamicSimulator(_dwc, engine ));
		Log::debugLog() << "Initializing simulator";
		try {
			State istate = initState;
			sim->init(istate);
		} catch(...){
			RW_THROW("could not initialize simulator!\n");
		}
		Log::debugLog() << "Creating Thread simulator";

		ThreadSimulator::Ptr tsim = ownedPtr( new ThreadSimulator(sim, initState) );
		ThreadSimulator::StepCallback cb( boost::bind(&OwnGraspTaskSimulator::stepCB, this, _1, _2) );

		tsim->setStepCallBack( cb );
		tsim->setPeriodMs(-1);
		tsim->setTimeStep(0.01);

		_simulators.push_back(tsim);
		_homeState = initState;
	}

	_initialized = true;
}


void OwnGraspTaskSimulator::load(const std::string& filename){
	Log::infoLog() << "Loading tasks: ";
	Log::infoLog() << "\t-Filename: " << filename;
	rwlibs::task::CartesianTask::Ptr task;
	try {
		XMLTaskLoader loader;
		loader.load( filename );
		task = loader.getCartesianTask();
	} catch (const Exception& exp) {
		RW_WARN("Unable to load tasks from file! " << filename);
		return;
	}
	load(task);
}

void OwnGraspTaskSimulator::load(rwlibs::task::CartesianTask::Ptr graspTasks){
	while(!_taskQueue.empty())
		_taskQueue.pop();
	_roottask = graspTasks;
	_taskQueue.push(_roottask);
	_currentTask = NULL;

	int nrOfTargets = 0;
	std::stack<rwlibs::task::CartesianTask::Ptr> tmpStack;
	tmpStack.push(graspTasks);
	while(!tmpStack.empty()){
		rwlibs::task::CartesianTask::Ptr tmpTask = tmpStack.top();
		tmpStack.pop();

		nrOfTargets += tmpTask->getTargets().size();

		//BOOST_FOREACH(CartesianTarget::Ptr target, tmpTask->getTargets()){
		//    _alltargets.push_back( std::make_pair(tmpTask,target));
		//}

		BOOST_FOREACH(rwlibs::task::CartesianTask::Ptr subtask, tmpTask->getTasks()){
			tmpStack.push(subtask);
		}
	}
	_currentTargetIndex = 0;
	_totalNrOfExperiments = nrOfTargets;

	_objects = _dwc->findBodies<RigidBody>();
	std::string handName = _roottask->getPropertyMap().get<std::string>("Gripper");
	_dhand = _dwc->findDevice(handName);
	if(_dhand==NULL)
		RW_THROW("No such gripper in workcell: " << handName);
	_rhand = dynamic_cast<RigidDevice*>(_dhand);
	_hand = _dhand->getKinematicModel();
	_gripperDim = _hand->getDOF();

	// hbase is the
	_hbase = dynamic_cast<KinematicBody*>( _dhand->getBase() );
	if(_hbase==NULL)
		RW_THROW("The gripper base must be a KinematicBody: " << handName);
	_mbase = _hbase->getMovableFrame();

	std::string controllerName = _roottask->getPropertyMap().get<std::string>("ControllerName", "GraspController");
	SimulatedController* scontroller = _dwc->findController(controllerName).get();
	if(scontroller==NULL)
		RW_THROW("No controller exist with the name: " << controllerName);
	_graspController = dynamic_cast<rwlibs::control::JointController*>( scontroller->getController() );
	if(_graspController==NULL)
		RW_THROW("Only JointControllers are valid graspcontrollers!");

	std::string tcpName = _roottask->getPropertyMap().get<std::string>("TCP");
	_tcp = _dwc->getWorkcell()->findFrame(tcpName);

	Log::infoLog() << "LOAD TASKS DONE, nr of tasks: " << nrOfTargets;
}

//----- simulation control and query function api
void OwnGraspTaskSimulator::startSimulation(const rw::kinematics::State& initState){
	RW_WARN("1");
	if(!_initialized)
		init(_dwc, initState);

	if(_totalNrOfExperiments==0)
		RW_THROW("there are no tasks to simulate!");

	_requestSimulationStop = false;

	_failed = 0;
	_success = 0;
	_slipped = 0;
	_collision = 0;
	_timeout = 0;
	_simfailed = 0;
	_skipped = 0;
	_nrOfExperiments = 0;
	_lastSaveTaskIndex = 0;

	// remove all sensors if there are any
	// first remove any SimTaskObjectSensor from the simulator
	for(size_t i=0;i<_simulators.size();i++){
		if(_simStates.find(_simulators[i])!= _simStates.end() ){
			BOOST_FOREACH(BodyContactSensor::Ptr sensor, _simStates[_simulators[i]]._bsensors){
				_simulators[i]->getSimulator()->removeSensor(sensor);
			}
		}
	}

	_simStates.clear();

	while(!_taskQueue.empty())
		_taskQueue.pop();
	_taskQueue.push(_roottask);
	_currentTask = NULL;
	_currentTargetIndex = 0;
	_homeState = initState;


	// FOR NOW WE ONLY USE ONE THREAD
	std::cout << "simsize: "<< _simulators.size() << std::endl;
	for(size_t i=0;i<_simulators.size();i++){
		DynamicSimulator::Ptr sim = _simulators[i]->getSimulator();
		SimState sstate;
		sstate._state = _homeState;

		_hbase->getMovableFrame()->setTransform(Transform3D<>(Vector3D<>(100,100,100)), sstate._state);

		for(size_t j=0;j<_objects.size();j++){
			sstate._bsensors.push_back( ownedPtr(new BodyContactSensor("SimTaskObjectSensor", _objects[j]->getBodyFrame())) );
			sim->addSensor( sstate._bsensors.back() , sstate._state);
		}

		_simulators[i]->setPeriodMs(-1);
		_simulators[i]->setTimeStep(0.01);
		_simStates[_simulators[i]] = sstate;
	}
	std::cout << "STARTING SIMULATORS" << std::endl;
	for(size_t i=0;i<_simulators.size();i++){
		std::cout << "STARTING SIMULATOR: " << i << std::endl;
		_simulators[i]->start();
	}
	std::cout << "all started " << std::endl;
}

void OwnGraspTaskSimulator::pauseSimulation(){
	_requestSimulationStop = true;
	BOOST_FOREACH(ThreadSimulator::Ptr tsim, _simulators){
		tsim->stop();
	}
}

void OwnGraspTaskSimulator::resumeSimulation(){
	_requestSimulationStop = false;
	BOOST_FOREACH(ThreadSimulator::Ptr tsim, _simulators){
		tsim->start();
	}
}

bool OwnGraspTaskSimulator::isRunning(){

	BOOST_FOREACH(ThreadSimulator::Ptr tsim, _simulators){
		if(tsim->isRunning())
			return true;
	}
	return false;
}

bool OwnGraspTaskSimulator::isFinished(){
	//std::cout << "_totalNrOfExperiments==_nrOfExperiments" << _totalNrOfExperiments<<"=="<<_nrOfExperiments << "\n";
	return _totalNrOfExperiments==_nrOfExperiments;
}

size_t OwnGraspTaskSimulator::getNrTargets(){
	return _totalNrOfExperiments;
}

ThreadSimulator::Ptr OwnGraspTaskSimulator::getSimulator(){
	if(_simulators.size()>0)
		return _simulators[0];
	return NULL;
}

std::vector<ThreadSimulator::Ptr> OwnGraspTaskSimulator::getSimulators(){
	return _simulators;
}

int OwnGraspTaskSimulator::getNrTargetsDone(){
	return _nrOfExperiments;
}

//void OwnGraspTaskSimulator::initialize(){}

void OwnGraspTaskSimulator::stepCB(ThreadSimulator* sim, const rw::kinematics::State& state){
	SimState &sstate = _simStates[sim];
	//std::cout <<sim->getTime() << "   state: " << sstate._currentState << std::endl;
	int delay = _stepDelayMs;
	//_simTime = sim->getTime();
	if( delay!= 0 )
		TimerUtil::sleepMs(delay);
	if( _requestSimulationStop ){
		return;
	}


	sstate._state = state;

	Q currentQ = _hand->getQ(state);

	if(sstate._wallTimer.getTime()>60){ //seconds
		_timeout++;
		sstate._target->getPropertyMap().set<Q>("GripperConfiguration", currentQ);
		sstate._target->getPropertyMap().set<int>("TestStatus", TimeOut);
		sstate._currentState = NEW_GRASP;
	}

	if(sim->getTime()>20.0 && sstate._currentState != NEW_GRASP){
		_timeout++;
		sstate._target->getPropertyMap().set<Q>("GripperConfiguration", currentQ);
		sstate._target->getPropertyMap().set<int>("TestStatus", TimeOut);
		sstate._currentState = NEW_GRASP;
	}

	//Transform3D<> cT3d = Kinematics::worldTframe(_object->getBodyFrame(), state);
	if( sim->isInError() ) {
		// the simulator is in error, reinitialize or fix the error
		_simfailed++;
		//std::cout << "SIMULATION FAILURE0: " << std::endl;
		sstate._target->getPropertyMap().set<Q>("GripperConfiguration", currentQ);
		sstate._target->getPropertyMap().set<int>("TestStatus", SimulationFailure);
		//_restObjState = state;
		for(size_t i=0; i<_objects.size(); i++){
			Transform3D<> restTransform = Kinematics::frameTframe(_mbase, _objects[i]->getBodyFrame(), state);
			sstate._target->getPropertyMap().set<Transform3D<> >("GripperTObject"+boost::lexical_cast<std::string>(i), restTransform);
		}
		sstate._currentState = NEW_GRASP;
	}

	if(sstate._currentState!=NEW_GRASP ){
		if( getMaxObjectDistance( _objects, _homeState, state) > _maxObjectGripperDistanceThreshold ){
			//double mdist=getMaxObjectDistance( _objects, _homeState, state);
			_simfailed++;
			//std::cout <<sim->getTime() << " : ";
			//std::cout << "TASK FAILURE1: " << mdist << ">" << 0.5 << std::endl;
			sstate._target->getPropertyMap().set<Q>("GripperConfiguration", currentQ);
			sstate._target->getPropertyMap().set<int>("TestStatus", SimulationFailure);
			for(size_t i=0; i<_objects.size(); i++){
				Transform3D<> restTransform = Kinematics::frameTframe(_mbase, _objects[i]->getBodyFrame(), state);
				sstate._target->getPropertyMap().set<Transform3D<> >("GripperTObject"+boost::lexical_cast<std::string>(i), restTransform);
			}
			sstate._currentState = NEW_GRASP;
		}
	}

	if(sstate._currentState ==APPROACH){
		Transform3D<> ct3d = Kinematics::worldTframe(_mbase, state);
		bool isLifted = MetricUtil::dist2( ct3d.P(), sstate._wTmbase_approachTarget.P() )<0.002;
		//std::cout << MetricUtil::dist2( ct3d.P(), _approach.P() ) << " < " << 0.002 << std::endl;
		//if(sim->getTime()>1.2){

		//std::cout << "APPROACH: " << std::endl;
		if(isLifted){
			//std::cout << "GRASPING" << std::endl;
			_graspController->setTargetPos(sstate._closeQ);
			sstate._currentState=GRASPING;
			sstate._approachedTime = sim->getTime();
			sstate._restingTime = sstate._approachedTime;

			Transform3D<> t3d  = Kinematics::frameTframe(_tcp, _objects[0]->getBodyFrame(), state);
			sstate._target->getPropertyMap().set<Transform3D<> > ("ObjectTtcpApproach", inverse(t3d) );
		}
	}


	//std::cout << "step callback" << std::endl;
	if(sstate._currentState==GRASPING){
		//std::cout << "grasping" << std::endl;
		if(sim->getTime()> sstate._approachedTime+0.2){
			// test if the grasp is in rest
			bool isResting = DynamicUtil::isResting(_dhand, state, 0.02);
			//std::cout << isResting << "&&" << sim->getTime() << "-" << _restingTime << ">0.08" << std::endl;
			// if it is in rest then lift object
			if( (isResting && ( (sim->getTime()-sstate._restingTime)>0.4)) || sim->getTime()>10 ){
				// remember to check the transform of object relative to gripper
				//_restObjTransform = Kinematics::frameTframe(_mbase, _object->getBodyFrame(), state);
				sstate._graspTime = sim->getTime();
				sstate._postLiftObjState = state;
				//_objectBeginLift = _object->getBodyFrame()->getTransform(state);
				// now instruct the RigidBodyController to move the object to the home configuration

				//_mbase->setTransform(_home, nstate);
				sstate._target->getPropertyMap().set<Q>("GripperConfiguration", currentQ);
				for(size_t i=0;i<_objects.size();i++){
					sstate._target->getPropertyMap().set<Transform3D<> >("GripperTObject"+boost::lexical_cast<std::string>(i),
							_objects[i]->getTransformW(state));
				}
				GraspedObject gobj = getObjectContacts(state, sstate);
				if( gobj.object == NULL ){
					_failed++;
					//std::cout << "NEW_GRASP" << std::endl;
					//std::cout << "ObjectMissed" << std::endl;
					sstate._target->getPropertyMap().set<int>("TestStatus", ObjectMissed);
					sstate._target->getPropertyMap().set<Q>("QualityBeforeLifting", Q::zero(NR_OF_QUALITY_MEASURES));
					sstate._currentState = NEW_GRASP;
				} else {
					//std::cout << "LIFTING" << std::endl;
					State nstate = state;
					Q qualities = calcGraspQuality(state, sstate);
					sstate._target->getPropertyMap().set<Q>("QualityBeforeLifting", qualities);
					// Added by Thomas:
					int contactSize = gobj.contacts.size();
					sstate._target->getPropertyMap().set<int>("ContactPoints", contactSize);
					int i = 0;
					BOOST_FOREACH(Contact3D contact, gobj.contacts) {
						sstate._target->getPropertyMap().set<Vector3D<> >("ContactPointPos"+boost::lexical_cast<std::string>(i), contact.p);
						sstate._target->getPropertyMap().set<Vector3D<> >("ContactPointNormal"+boost::lexical_cast<std::string>(i), contact.n);
						sstate._target->getPropertyMap().set<double >("ContactPointNormalForce"+boost::lexical_cast<std::string>(i), contact.normalForce);
						i++;
					}
					Transform3D<> ct3d = Kinematics::worldTframe(_dhand->getBase()->getBodyFrame(), state);
					/*Vector3D<> acc = _roottask->getPropertyMap().get<Vector3D<> >("AccWGripperClosed", Vector3D<>::zero());
					for(size_t i=0; i<_objects.size(); i++) {
						Vector3D<> force = acc*_objects[i]->getInfo().mass;
						force = ct3d.R()*force;
						std::cout <<_objects[i]->getForceW(nstate);
						_objects[i]->setForceW(force, nstate);
						std::cout << " to " <<_objects[i]->getForceW(nstate) << std::endl;
					}*/
					// Added section ended

					sim->getSimulator()->setTarget(_dhand->getBase(), sstate._wTmbase_retractTarget, nstate);
					sim->reset(nstate);
					_delayCounter = 0;
					sstate._currentState = LIFTING;
				}
			}
			if( !isResting ){
				sstate._restingTime = sim->getTime();
			}

		} else {
			sstate._restingTime = sim->getTime();
		}
	}

	if(sstate._currentState==LIFTING){
		_delayCounter++;
		// test if object has been lifted
		bool isLifted = true;
		Transform3D<> ct3d = Kinematics::worldTframe(_dhand->getBase()->getBodyFrame(), state);
		isLifted &= MetricUtil::dist2( ct3d.P(), sstate._wTmbase_retractTarget.P() )<0.005;

		Vector3D<> acc = _roottask->getPropertyMap().get<Vector3D<> >("AccWGripperClosed", Vector3D<>::zero());
		State nstate = state;
		for(size_t i=0; i<_objects.size(); i++) {
			Vector3D<> force = acc*_objects[i]->getInfo().mass;
			force = ct3d.R()*force;
			_objects[i]->setForceW(force, nstate);
		}
		sim->setState(nstate);

		//isLifted &= ct3d.R().equal(_home.R(),0.01);
		//std::cout << MetricUtil::dist2( ct3d.P(), _home.P() ) << "<" << 0.001 << std::endl;
		// if its lifted then verify the object gripper transform
		if (isLifted && _delayCounter >= 10) {
			GraspedObject gobj = getObjectContacts(state, sstate);
			//getTarget()->getPropertyMap().set<Transform3D<> > ("GripperTObject", t3d);
			if( gobj.object == NULL ){
				//std::cout << "No contacts!" << std::endl;
				_failed++;
				sstate._target->getPropertyMap().set<int> ("TestStatus", ObjectDropped);
				sstate._target->getPropertyMap().set<int> ("LiftStatus", ObjectDropped);
				sstate._target->getPropertyMap().set<Q>("QualityAfterLifting", Q::zero(2));
			} else {
				sstate._target->getPropertyMap().set<int> ("LiftStatus", Success);
				Q qualities = calcGraspQuality(state, sstate);
				sstate._target->getPropertyMap().set<Q>("QualityAfterLifting", qualities);

				Transform3D<> t3d = Kinematics::frameTframe(_mbase, gobj.object->getBodyFrame(), state);

				// Test the success of lifting the object.
				// We need to look at the objects that are actually touching
				Body* object = gobj.object;
				Body* gripperBody = gobj.bodies[0];

				Transform3D<> tcpTo_before = Kinematics::frameTframe(_tcp, object->getBodyFrame(), sstate._postLiftObjState);
				Transform3D<> tcpTo_after  = Kinematics::frameTframe(_tcp, object->getBodyFrame(), state);
				sstate._target->getPropertyMap().set<Transform3D<> > ("ObjectTtcpGrasp", inverse(tcpTo_before) );

				rw::math::RPY<> rpy(inverse(tcpTo_before).R());
				sstate._target->getPropertyMap().set<RPY<> >("ObjectTtcpGraspRPY", rpy);
				sstate._target->getPropertyMap().set<Transform3D<> > ("tcpGraspTObject",tcpTo_before );
				rw::math::RPY<> rpy2(tcpTo_before.R());
				sstate._target->getPropertyMap().set<RPY<> >("tcpGraspTObjectRPY", rpy2);

				sstate._target->getPropertyMap().set<Transform3D<> > ("ObjectTtcpLift", inverse(tcpTo_after) );


				Transform3D<> oTg_before = Kinematics::frameTframe(object->getBodyFrame(), gripperBody->getBodyFrame(), sstate._postLiftObjState);
				Transform3D<> oTg_after  = Kinematics::frameTframe(object->getBodyFrame(), gripperBody->getBodyFrame(), state);
				Vector3D<> slipVector = oTg_after.P() - oTg_before.P();
				// allow op to 2 cm slip else its a fault

				double slippage = slipVector.norm2();

				double liftResult;
				if(slippage <= 0.02)
					liftResult = (0.02 - slippage)*50;
				else
					liftResult = 0.0;
				//std::cout << "Slippage: " << slippage <<" " << object->getName()<<" " << gripperBody->getName() << std::endl;
				//std::cout << "LIFT RESULTS: " << liftResult << std::endl;
				sstate._target->getPropertyMap().set<double> ("LiftResult", liftResult);

				if (liftResult == 0.0) {
					_failed++;
					sstate._target->getPropertyMap().set<int> ("TestStatus", ObjectDropped);
					//std::cout << sim->getTime() << " : " << "ObjectDropped" << std::endl;
				} else if (liftResult > 0.50) { // At most 1cm difference with hand lift
					_success++;
					sstate._target->getPropertyMap().set<int> ("TestStatus", Success);
					//std::cout << sim->getTime() << " : " << "Success" << std::endl;
				} else {
					_slipped++;
					sstate._target->getPropertyMap().set<int> ("TestStatus", ObjectSlipped);
					//std::cout << sim->getTime() << " : " << "ObjectSlipped" << std::endl;
				}

			}
			sstate._target->getPropertyMap().set<Q>("GripperConfigurationPost", currentQ);
			for(size_t i=0;i<_objects.size();i++){
				sstate._target->getPropertyMap().set<Transform3D<> >("GripperTObjectLift"+boost::lexical_cast<std::string>(i),
						_objects[i]->getTransformW(state));
			}
			sstate._currentState = NEW_GRASP;
		}
	}

	if(sstate._currentState==NEW_GRASP){
		State nstate = _homeState;
		// pop new task from queue
		// if all tasks
		//RW_WARN("1");
		bool colFreeSetup = false;
		do{
			//RW_WARN("1");
			if( !getNextTarget(sstate) ){
				// end we are done with this threadsimulator
				std::cout << "STOP" << std::endl;
				sstate._stopped = true;
				sim->postStop();
				// stop the thread

				return;
			}
			//RW_WARN("1");

			if( sstate._target->getPropertyMap().get<int>("TestStatus",-1)>=0 ){
				// if test status is set then we allready processed this task.
				_skipped++;
				std::cout << "SKIPPING TARGET - allready processed!\n";
				continue;
			}

			Transform3D<> wTref = Kinematics::worldTframe(sstate._taskRefFrame, _homeState);
			Transform3D<> refToffset = sstate._taskOffset;
			Transform3D<> offsetTtarget = sstate._target->get();
			Transform3D<> mbaseTtcp = Kinematics::frameTframe(_mbase, _tcp, _homeState);
			Transform3D<> wTmparent = Kinematics::worldTframe(_mbase->getParent(_homeState), _homeState);
			//std::cout << "mbase parent: "<< _mbase->getParent(_homeState)->getName() << std::endl;
			//RW_WARN("1");
			// and calculate the home lifting position
			sstate._wTtcp_initTarget = wTref * refToffset * offsetTtarget;
			sstate._wTmbase_initTarget     = sstate._wTtcp_initTarget * inverse(mbaseTtcp);
			sstate._wTmbase_approachTarget = sstate._wTtcp_initTarget * sstate._approach * inverse(mbaseTtcp);
			sstate._wTmbase_retractTarget  = sstate._wTtcp_initTarget * sstate._approach * sstate._retract * inverse(mbaseTtcp);
			//RW_WARN("1");
			// we initialize the transform
			//std::cout << "inverse(wTmparent)" << inverse(wTmparent) << std::endl;

			//std::cout << "inverse(wTmparent)" << sstate._wTmbase_initTarget << std::endl;
			//std::cout << "inverse(wTmparent)" << sstate._wTmbase_initTarget << std::endl;
			_mbase->setTransform( inverse(wTmparent) * sstate._wTmbase_initTarget, nstate);
			//RW_WARN("1");

			_hand->setQ(sstate._openQ, nstate);
			for(size_t i=0;i<_objects.size();i++){
				Transform3D<> tobj = _objects[i]->getMovableFrame()->getTransform(_homeState);
				_objects[i]->getMovableFrame()->setTransform(tobj, nstate);
			}
			//RW_WARN("1");
			// set max force
			if(_rhand ){
				Q forceLim = sstate._task->getPropertyMap().get<Q>("TauMax",Q());
				if(forceLim.size()>0)
					_rhand->setForceLimit(forceLim);
			}
			//RW_WARN("1");
			colFreeSetup = !_collisionDetector->inCollision(nstate, NULL, true);
			//RW_WARN("1");
			if( !colFreeSetup ){
				sstate._target->getPropertyMap().set<int>("TestStatus", CollisionInitially);
				sstate._target->getPropertyMap().set<Q>("GripperConfiguration", sstate._openQ);

				for(size_t i=0;i<_objects.size();i++){
					sstate._target->getPropertyMap().set<Transform3D<> >("GripperTObject"+boost::lexical_cast<std::string>(i),
							_objects[i]->getTransformW(state));
				}

				//std::cout << "0.0 : InCollision " << std::endl;
				_collision++;
			}
			//RW_WARN("1");
			//std::cout << "1:" << _collision << _nrOfExperiments<< std::endl;

			_nrOfExperiments++;
		} while( !colFreeSetup );

		if( _nrOfExperiments > _lastSaveTaskIndex+_autoSaveInterval ){
			//saveTasks(true);
			_lastSaveTaskIndex = _nrOfExperiments;
		}
		// reset simulation
		_dhand->getBase()->reset(nstate);
		// Added
		//Vector3D<> acc = _roottask->getPropertyMap().get<Vector3D<> >("AccWGripperOpen", Vector3D<>::zero());
		for(size_t i=0; i<_objects.size(); i++) {
			//Vector3D<> force = acc*_objects[i]->getInfo().mass;
			//_objects[i]->setForce(force, nstate);
			_objects[i]->setForce(Vector3D<>::zero(), nstate);
		}
		// End added
		sim->reset(nstate);
		//sim->setPeriodMs(-1);
		//sim->setTimeStep(0.01);
		sim->getSimulator()->disableBodyControl();
		sim->getSimulator()->setTarget(_dhand->getBase(), sstate._wTmbase_approachTarget, nstate);
		_graspController->setTargetPos(sstate._openQ);
		sstate._wallTimer.resetAndResume();
		sstate._currentState = APPROACH;
		Transform3D<> t3d  = Kinematics::frameTframe(_tcp, _objects[0]->getBodyFrame(), nstate);
		sstate._target->getPropertyMap().set<Transform3D<> > ("ObjectTtcpTarget", inverse(t3d) );

		sstate._restingTime = 0;
	}
}


namespace {

void exportMathematica(std::ostream& outfile, CartesianTask::Ptr tasks, int gripperDim) {
	outfile << "// Description: {target.pos(3), target.rpy(3), TestStatus(1), GripperConfiguration("<<gripperDim<<"), "
			"GripperTObject.pos, GripperTObject.rpy, ObjectTtcpBefore.pos, ObjectTtcpBefore.rpy, ObjectTtcpAfter.pos, ObjectTtcpAfter.rpy}\n";
	outfile << "// TestStatus enum { UnInitialized=0, Success=1, CollisionInitially=2, ObjectMissed=3, ObjectDropped=4, ObjectSlipped=5, TimeOut=6, SimulationFailure=7}\n";
	std::stack<CartesianTask::Ptr> taskStack;
	taskStack.push(tasks);
	while(!taskStack.empty()){
		CartesianTask::Ptr task = taskStack.top();
		taskStack.pop();

		std::vector<CartesianTarget::Ptr> targets = task->getTargets();
		outfile<<"{" << task->getId() << "}\n";
		BOOST_FOREACH(CartesianTarget::Ptr target, targets) {
			const Vector3D<>& pos = target->get().P();
			const RPY<> rpy(target->get().R());
			int status = target->getPropertyMap().get<int>("TestStatus", OwnGraspTaskSimulator::UnInitialized);
			outfile<<"{"<<pos(0)<<","<<pos(1)<<","<<pos(2)<<","<<rpy(0)<<","<<rpy(1)<<","<<rpy(2)<<","<<status<<",";

			Q distance = target->getPropertyMap().get<Q>("GripperConfiguration", Q::zero(gripperDim));
			for(size_t i=0;i<distance.size();i++)
				outfile << distance[i] << ",";

			Transform3D<> t3d = target->getPropertyMap().get<Transform3D<> >("GripperTObject", Transform3D<>::identity());
			RPY<> rpyObj(t3d.R());
			outfile << t3d.P()[0] << "," << t3d.P()[1] << "," <<t3d.P()[2] << ","
					<< rpyObj(0) << "," << rpyObj(1) << "," <<rpyObj(2) << ",";

			t3d = target->getPropertyMap().get<Transform3D<> >("ObjectTtcpTarget", Transform3D<>::identity() );
			rpyObj = RPY<>(t3d.R());
			outfile << t3d.P()[0] << "," << t3d.P()[1] << "," <<t3d.P()[2] << ","
					<< rpyObj(0) << "," << rpyObj(1) << "," <<rpyObj(2)<< ",";

			t3d = target->getPropertyMap().get<Transform3D<> >("ObjectTtcpApproach", Transform3D<>::identity() );
			rpyObj = RPY<>(t3d.R());
			outfile << t3d.P()[0] << "," << t3d.P()[1] << "," <<t3d.P()[2] << ","
					<< rpyObj(0) << "," << rpyObj(1) << "," <<rpyObj(2)<< ",";

			t3d = target->getPropertyMap().get<Transform3D<> >("ObjectTtcpGrasp", Transform3D<>::identity() );
			rpyObj = RPY<>(t3d.R());
			outfile << t3d.P()[0] << "," << t3d.P()[1] << "," <<t3d.P()[2] << ","
					<< rpyObj(0) << "," << rpyObj(1) << "," <<rpyObj(2) << ",";

			t3d = target->getPropertyMap().get<Transform3D<> >("ObjectTtcpLift", Transform3D<>::identity() );
			rpyObj = RPY<>(t3d.R());
			outfile << t3d.P()[0] << "," << t3d.P()[1] << "," <<t3d.P()[2] << ","
					<< rpyObj(0) << "," << rpyObj(1) << "," <<rpyObj(2) << "}"<< "\n";
		}

	}

}

}

void OwnGraspTaskSimulator::save(const std::string& filename, CartesianTask::Ptr tasks, ExportFormat format){
	std::ofstream outfile(filename.c_str());
	save(outfile, tasks, format);
	outfile.close();
}

void OwnGraspTaskSimulator::save(std::ostream& ostr, CartesianTask::Ptr tasks, ExportFormat format){
	int gripperDim = 0;
	if( format==TaskFormat ){
		try {
			XMLTaskSaver saver;
			saver.save(tasks, ostr );
		} catch (const Exception& exp) {
			RW_THROW("Unable to save task: " << exp.what());
		}
	} else if( format==CommaSeperatedFormat ){
		exportMathematica(ostr, tasks, gripperDim);
	} else {
		RW_THROW("Unkown Export Format!");
	}
}



std::vector<rw::sensor::Contact3D> OwnGraspTaskSimulator::getObjectContacts(const rw::kinematics::State& state,
		RigidBody *object,
		BodyContactSensor::Ptr sensor,
		std::vector<Body*>& gripperbodies )
{
	const std::vector<rw::sensor::Contact3D>& contacts = sensor->getContacts();
	const std::vector<Body*>& bodies = sensor->getBodies();

	RW_ASSERT(bodies.size() == contacts.size() );
	//std::cout << "nr contacts: " << contacts.size() << " body: " << object->getName() << std::endl;
	std::vector<rw::sensor::Contact3D> contactres;
	std::map<std::string, Frame*> frameTree = Kinematics::buildFrameMap( *_hand->getBase(),  state);
	frameTree[_hand->getBase()->getName()] = _hand->getBase();
	for(size_t i=0; i<bodies.size(); i++){
		if( bodies[i]!=NULL ){
			// test that the body frame is part of the gripper
			//std::cout << "Body: " << bodies[i]->getBodyFrame()->getName() << std::endl;
			if( frameTree.find(bodies[i]->getBodyFrame()->getName() ) != frameTree.end() ){
				if(contacts[i].normalForce>0.0001){
					contactres.push_back(contacts[i]);
					//contactres.back().p[0] = object->getInfo().masscenter[0];
					contactres.back().mu = _dwc->getMaterialData().getFrictionData(object->getMaterialID(),bodies[i]->getMaterialID()).parameters[0].second(0);
					// allso save the body of the gripper that is in contact
					if(std::find(gripperbodies.begin(), gripperbodies.end(), bodies[i]) == gripperbodies.end() )
						gripperbodies.push_back(bodies[i]);
				}
			}
		} else {
			//std::cout << "Body: NULL" << std::endl;
		}
	}
	//std::cout << "Get CONTACTS " << contacts.size() << " --> " << contactres.size() << std::endl;
	return contactres;
}

OwnGraspTaskSimulator::GraspedObject OwnGraspTaskSimulator::getObjectContacts(const rw::kinematics::State& state, SimState &sstate)
{
	std::vector<GraspedObject> result;
	for(size_t i=0; i<_objects.size();i++){
		GraspedObject obj;
		obj.object = _objects[i];
		obj.contacts = getObjectContacts(state, _objects[i], sstate._bsensors[i], obj.bodies);
		if(obj.contacts.size()>0)
			result.push_back(obj);
	}
	if(result.size()==0)
		return GraspedObject();
	int bestIdx = 0;
	for(size_t i=1;i<result.size();i++){
		if( result[i].contacts.size() > result[bestIdx].contacts.size() )
			bestIdx = i;
	}
	GraspedObject res = result[bestIdx];
	std::vector<Contact3D> cont = res.contacts;
	std::vector<Contact3D> newCont;
	for (size_t i = 0; i < cont.size(); i++) {
		std::vector<Contact3D>::iterator pos = newCont.begin();
		bool stop = false;
		while (!stop && pos != newCont.end()) {
			if (cont[i].normalForce > (*pos).normalForce)
				stop = true;
			else if (cont[i].normalForce < (*pos).normalForce)
				pos++;
			else {
				if (cont[i].p[0] > (*pos).p[0])
					stop = true;
				else if (cont[i].p[0] < (*pos).p[0])
					pos++;
				else {
					if (cont[i].p[1] > (*pos).p[1])
						stop = true;
					else if (cont[i].p[1] < (*pos).p[1])
						pos++;
					else {
						if (cont[i].p[2] >= (*pos).p[2])
							stop = true;
						else pos++;
					}
				}
			}
		}
		newCont.insert(pos, cont[i]);
	}
	res.contacts = newCont;

	/*double maxOrigForce = 0;
	double totalForce = 0;
	std::vector<Contact3D> cont2 = res.contacts;
	// Merge contact points in two bins
	if (res.contacts.size() > 2) {
		Contact3D finalContact1 = cont2[0];
		Contact3D finalContact2 = cont2[1];
		double forceDiff = 9999.9;
		double dist = 0;
		for (size_t j = 0; j < cont2.size(); j++) {
			if (cont2[j].normalForce > maxOrigForce) maxOrigForce = cont2[j].normalForce;
			Vector3D<> normal = cont2[j].n;
			Contact3D contact1;// = cont2[j];
			Contact3D contact2;// = cont2[j];
			contact1.mu = cont2[j].mu;
			contact1.n = Vector3D<>::zero();
			contact1.p = Vector3D<>::zero();
			contact1.f = Vector3D<>::zero();
			contact1.normalForce = 0;
			contact2.mu = cont2[j].mu;
			contact2.n = Vector3D<>::zero();
			contact2.p = Vector3D<>::zero();
			contact2.f = Vector3D<>::zero();
			contact2.normalForce = 0;
			int n1 = 0;
			int n2 = 0;
			for (size_t i = 0; i < cont2.size(); i++) {
				double dir = cont2[i].n[0]*normal[0]+cont2[i].n[1]*normal[1]+cont2[i].n[2]*normal[2];
				int n;
				if (dir > 0) {
					n = n1++;
					contact1.normalForce = contact1.normalForce + cont2[i].normalForce;
					//contact1.n = (contact1.n*n + res.contacts[i].n)/(n+1);
					contact1.p = (contact1.p*n + cont2[i].p)/(n+1);
					//contact1.f = (contact1.f*n + res.contacts[i].f)/(n+1);
				} else {
					n = n2++;
					contact2.normalForce = contact2.normalForce + cont2[i].normalForce;
					//contact2.n = (contact2.n*n + res.contacts[i].n)/(n+1);
					contact2.p = (contact2.p*n + cont2[i].p)/(n+1);
					//contact2.f = (contact2.f*n + res.contacts[i].f)/(n+1);
				}
			}
			double diff = contact1.normalForce-contact2.normalForce;
			if (diff < 0) diff = -diff;
			double d = (contact1.p-contact2.p).norm2();
			if (diff < forceDiff && d > dist) {
				contact1.n = normal;
				contact1.f = normal*contact1.normalForce;
				contact2.n = -normal;
				contact2.f = -normal*contact2.normalForce;
				finalContact1 = contact1;
				finalContact2 = contact2;
				forceDiff = diff;
				dist = d;
				totalForce = contact1.normalForce+contact2.normalForce;
				totalForce /= 2;
			}
		}
		std::vector<Contact3D> newCont2;
		newCont2.push_back(finalContact1);
		newCont2.push_back(finalContact2);
		//res.contacts = newCont2;
	}*/


	if (res.contacts.size() > 2) {
		double maxOrigForce = 1000;
		//double maxOrigForce = 0;
		double totalForce = 0;
		for (size_t j = 0; j < res.contacts.size(); j++) {
			//if (res.contacts[j].normalForce > maxOrigForce) maxOrigForce = res.contacts[j].normalForce;
			if (res.contacts[j].normalForce < maxOrigForce) maxOrigForce = res.contacts[j].normalForce;
			totalForce += res.contacts[j].normalForce;
		}
		totalForce /= 2;

		double scale = totalForce / maxOrigForce;
		for (size_t j = 0; j < res.contacts.size(); j++) {
			res.contacts[j].normalForce *= scale;
			res.contacts[j].f*= scale;
		}
	}

	// Merge contact points that are too close
	/*double epsilon = 0.005;
	std::vector<Contact3D> newCont2;
	for (size_t i = 0; i < newCont.size(); i++) {
		bool merged = false;
		for (size_t j = 0; j < newCont2.size() && !merged; j++) {
			Vector3D<> diffVec = (newCont[i].p-newCont2[j].p);
			double diff = diffVec.norm2();
			if (diff < epsilon) {
				Contact3D newContact = newCont2[j];
				newContact.normalForce += newCont[i].normalForce;
				newContact.n = (newContact.n + newCont[i].n)/2;
				newContact.p = (newContact.p + newCont[i].p)/2;
				newContact.f = (newContact.f + newCont[i].f)/2;
				newCont2[j] = newContact;
				merged = true;
			}
		}
		if (!merged) {
			newCont2.push_back(newCont[i]);
		}
	}
	res.contacts = newCont2;*/

	return res;
	//return result[bestIdx];
}


rw::math::Q OwnGraspTaskSimulator::calcGraspQuality(const State& state, SimState &sstate){
	GraspedObject gobj = getObjectContacts(state, sstate);
	std::vector<Contact3D> contacts = gobj.contacts;
	RigidBody *object = gobj.object;
	// calculate grasp quality
	rw::math::Q qualities( Q::zero(NR_OF_QUALITY_MEASURES) );
	if(gobj.object==NULL || gobj.contacts.size()==0)
		return qualities;
	Grasp3D g3d( contacts );

	Math::seed(123);
	for (int i = 0; i < 1; i++) {
	if(g3d.contacts.size()<4){
		std::vector<Contact3D > cons = g3d.contacts;
		BOOST_FOREACH(Contact3D& c, cons){
			// add a small random value to normal and position
			c.n += Vector3D<>(Math::ran(-0.1,0.1), Math::ran(-0.1,0.1),Math::ran(-0.1,0.1));
			c.n = normalize(c.n);
			c.p += Vector3D<>(Math::ran(-0.002,0.002), Math::ran(-0.002,0.002),Math::ran(-0.002,0.002));
			g3d.contacts.push_back(c);
		}
	}
	}


	//std::cout << "***** NR OF CONTACTS IN GRASP: " << g3d.contacts.size() << std::endl;
	/*
    BOOST_FOREACH(Contact3D con, g3d.contacts){
        std::cout << "--- contact";
        std::cout << "\n-- nf: " << con.normalForce;
        std::cout << "\n-- mu: " << con.mu;
        std::cout << "\n-- n : " << con.n;
        std::cout << "\n-- p: " << con.p << std::endl;
    }
	 */

	Vector3D<> cm = object->getInfo().masscenter;
	double r = GeometryUtil::calcMaxDist( object->getGeometry(), cm);
	//std::cout << "cm    : " << cm << std::endl;
	//std::cout << "Radius: " << r<< std::endl;

	rw::graspplanning::OwnGWSMeasure3D wmeasure2( 10 , false);

	wmeasure2.setObjectCenter(cm);
	wmeasure2.setLambda(2*1/r);
	wmeasure2.quality(g3d);

	rw::graspplanning::OwnGWSMeasure3D wmeasure3( 10, true );
	wmeasure3.setObjectCenter(cm);
	wmeasure3.setLambda(2*1/r);
	wmeasure3.quality(g3d);

	//std::cout << "getvals " << r<< std::endl;
	//std::cout << "Wrench calc done!" << std::endl;
	qualities(0) = wmeasure2.getMinWrench();
	qualities(1) = wmeasure2.getAvgWrench();
	qualities(2) = wmeasure3.getMinWrench();
	qualities(3) = wmeasure3.getAvgWrench();

	//std::cout << "CMCPP " << r<< std::endl;
	CMDistCCPMeasure3D CMCPP( cm, r*2);
	qualities(4) = CMCPP.quality( g3d );
	//std::cout << "Quality: " << qualities << std::endl;
	return qualities;
}

bool OwnGraspTaskSimulator::getNextTarget(OwnGraspTaskSimulator::SimState& sstate){
	// were we iterate over all tasks and their targets
	//RW_WARN("1");

	if(_currentTask==NULL || ((size_t)_currentTargetIndex>=_currentTask->getTargets().size()-1)){
		//RW_WARN("1");
		if(_taskQueue.empty() ){
			//RW_WARN("1");
			_currentTask = NULL;
			return false;
		}


		// if the current target is the last target then get the next task
		_currentTask = _taskQueue.top();
		_taskQueue.pop();
		_currentTargetIndex=0;

		// push all task children on the queue
		for(int i=_currentTask->getTasks().size()-1; i>=0; i--){
			_taskQueue.push(_currentTask->getTasks()[i]);
		}

	} else {
		//RW_WARN("1");
		_currentTargetIndex++;
	}
	//RW_WARN("1");

	if(sstate._task!= _currentTask){
		//RW_WARN("1");
		sstate._task = _currentTask;
		// TODO: compute task temp variables

		std::string refframename = _currentTask->getPropertyMap().get<std::string>("refframe","WORLD");
		sstate._taskRefFrame = _dwc->getWorkcell()->findFrame<Frame>(refframename);
		RW_ASSERT(sstate._taskRefFrame);
		sstate._taskOffset = _currentTask->getPropertyMap().get<Transform3D<> >("Offset",Transform3D<>::identity() );
		sstate._approach = _currentTask->getPropertyMap().get<Transform3D<> >("Approach",Transform3D<>::identity() );
		sstate._retract = _currentTask->getPropertyMap().get<Transform3D<> >("Retract",Transform3D<>::identity() );

		sstate._openQ = _currentTask->getPropertyMap().get<Q>("OpenQ");
		sstate._closeQ = _currentTask->getPropertyMap().get<Q>("CloseQ");
	}
	//RW_WARN("1");
	// and target specific temp variables
	sstate._target = _currentTask->getTargets()[_currentTargetIndex];
	//RW_WARN("1");
	//sstate._wTtcp_initTarget,
	//                        _wTmbase_initTarget, // approach to this config from _initTarget
	//                        _wTmbase_approachTarget, // approach to this config from _initTarget
	//                        _wTmbase_retractTarget; // retract to this config from _approachTarget



	Log::infoLog() << "-- target nr: "<< std::setw(5) << _currentTargetIndex
			<< " success:"<< std::setw(5) << _success
			<< " slipped:" << std::setw(5) << _slipped
			<< " failed:" << std::setw(5) << _failed
			<< " collisions:" << std::setw(5) << _collision
			<< " timeouts:" << std::setw(5) << _timeout
			<< " skipped:" << std::setw(5) << _skipped
			<< " simfailures:" << std::setw(5) <<_simfailed << "\n";

	return true;
}

